#include <stdlib.h>

#include "gtest/gtest.h"

#include "slp_joint.hpp"
#include "projection_l1.hpp"
#include "filter.hpp"


#ifdef DOUBLE
#define FTYPE double
#define EPS 1e-13
#define EPSIT 1e-7
#endif

#ifdef SINGLE
#define FTYPE float
#define EPS 1e-5
#define EPSIT 1e-4
#endif

class SLPTest : public ::testing::Test{
protected:

  SLPTest(){
  }

  virtual ~SLPTest(){
  }

  virtual void SetUp(){
  }

  virtual void TearDown(){
  }
};


TEST_F(SLPTest, Projection_l1){

  int n = 20;
  FTYPE b1[] = {-0.947246643957371, 0.540149747070348, -0.216602140976276,
               1.189031974948345, 1.316987831810854, -0.405628869688033,
               -0.444906311105150, 1.328385815592680,  0.833802589212637,
               0.604446391975289, -0.106671545865946, 0.003015329404578,
               0.457111658357373, 0.921720899428106, -0.197341695987964,
               0.075503488519393, 0.008886920027922, 0.838831098830714,
               -0.542412761860117, 0.567566470034518};

  FTYPE b2[] = {0, 0, 0, 0, 0, 0,
               0, -1.0, 0, 0, 0, 0,
               0, 0, 1.2, 0, 0, 0,
                0, 0};

  FTYPE x[] = {-0.349689988237872, 0, 0, 0.591475319228845, 0.719431176091355,
               0, 0, 0.730829159873181, 0.236245933493137, 0.006889736255790,
               0, 0, 0, 0.324164243708607, 0, 0, 0, 0.241274443111214, 0, 0};

  FTYPE tau = 3.2;
  FTYPE * xh = vector(n);

  projection_l1(n, b1, tau, xh);

  for( int i = 0 ; i < n ; i++)
    ASSERT_NEAR(x[i], xh[i], EPS) << "Vectors x and xh differ at index " << i;

  projection_l1(n, b2, tau, xh);

  for( int i = 0 ; i < n ; i++)
    ASSERT_NEAR(b2[i], xh[i], EPS) << "Vectors x and xh differ at index " << i;

  del_vector(xh);

}


TEST_F(SLPTest, S) {

  int n = 5;
  FTYPE x[]   = {1.0, 2, -0.5, 1.2, -0.2};
  FTYPE t = 0.6;
  FTYPE y[] = {0.4, 1.4, 0.0, 0.6, 0.0};
  FTYPE yh[] = {0.0, 0.0, 0.0, 0.0, 0.0};
  
  S(n, x, t, yh);

  for( int i = 0 ; i < n ; i++)
    ASSERT_NEAR(y[i], yh[i], EPS) << "Vectors y and yh differ at index " << i;

}


TEST_F(SLPTest, filter) {

  int n = 4;
  int k = 10;
  FTYPE x[]   =  {1.0, 2.0, -0.5, 1.2, -0.2, 0.1, 0.6, 0.8, 0.1, -0.5};
  FTYPE a[] = {2.0, 4.0, -1.0, 1.0};
  FTYPE y[] = {2.0, 8.0, 6.0, -0.6, 6.9, -2.3, 3.0, 3.7, 2.9, -0.8};
  FTYPE yh[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

  filter(a, n, x, k, yh);

  for( int i = 0 ; i < k ; i++)
    ASSERT_NEAR(y[i], yh[i], EPS) << "Vectors y and yh differ at index " << i;


}

TEST_F(SLPTest, fftfilterfunc) {

  int n = 4;
  int k = 10;
  FTYPE x[]   =  {1.0, 2.0, -0.5, 1.2, -0.2, 0.1, 0.6, 0.8, 0.1, -0.5};
  FTYPE a[] = {2.0, 4.0, -1.0, 1.0};
  FTYPE y[] = {2.0, 8.0, 6.0, -0.6, 6.9, -2.3, 3.0, 3.7, 2.9, -0.8};
  FTYPE yh[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

  filter(a, n, x, k, yh);

  for( int i = 0 ; i < k ; i++)
    ASSERT_NEAR(y[i], yh[i], EPS) << "Vectors y and yh differ at index " << i;


}


TEST_F(SLPTest, fftfilter) {

  int n = 4;
  int k = 10;
  FTYPE x[]   =  {1.0, 2.0, -0.5, 1.2, -0.2, 0.1, 0.6, 0.8, 0.1, -0.5};
  FTYPE a[] = {2.0, 4.0, -1.0, 1.0};
  FTYPE y[] = {2.0, 8.0, 6.0, -0.6, 6.9, -2.3, 3.0, 3.7, 2.9, -0.8};
  FTYPE yh[] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};

  fftfilter * h = new fftfilter(n, x, k);

  h->filter(a, yh);
  for( int i = 0 ; i < k ; i++)
    ASSERT_NEAR(y[i], yh[i], EPS) << "Vectors y and yh differ at index " << i;

  delete h;
}

TEST_F(SLPTest, adjointfilter) {

  int l = 4;
  int k = 10;
  FTYPE x[] = {1.0, 2.0, -0.5, 1.2, -0.2, 0.1, 0.6, 0.8, 0.1, -0.5};
  FTYPE y[] = {2.0, 8.0, 6.0, -0.6, 6.9, -2.3, 3.0, 3.7, 2.9, -0.8};
  FTYPE a[] = {18.119999999999997, 33.800000000000004, -0.540000000000000,  17.019999999999996};
  FTYPE ah[] = {0.0, 0.0, 0.0, 0.0};

  adjointfilter(x, y, k, ah, l);

  for( int i = 0 ; i < l ; i++ )
    ASSERT_NEAR(a[i], ah[i], EPS) << "Vectors r and rh differ at index " << i;

}

TEST_F(SLPTest, fftadjointfilter) {

  int l = 4;
  int k = 10;
  FTYPE x[] = {1.0, 2.0, -0.5, 1.2, -0.2, 0.1, 0.6, 0.8, 0.1, -0.5};
  FTYPE y[] = {2.0, 8.0, 6.0, -0.6, 6.9, -2.3, 3.0, 3.7, 2.9, -0.8};
  FTYPE a[] = {18.119999999999997, 33.800000000000004, -0.540000000000000,  17.019999999999996};
  FTYPE ah[] = {0.0, 0.0, 0.0, 0.0};

  fftadjointfilter * h = new fftadjointfilter(l, x, k);

  h->filter(y, ah);

  for( int i = 0 ; i < l ; i++ )
    ASSERT_NEAR(a[i], ah[i], EPS) << "Vectors r and rh differ at index " << i;

  delete h;
}


TEST_F(SLPTest, SLPF) {

  int N = 7;
  FTYPE x[] = {0.0886221518141870,0.6100883986609733,-2.3085004845255379,
               -0.1766595206825720,1.6502980035082231,1.0484879097668793,
               -0.0877782968571668,2.7770486147025437,-0.3908467137394856,
               -0.8083623106713005,0.5207812917294559,0.9337442431429159,
               -1.0905918302151436,-0.5599398028754168};
  FTYPE z[] = {0.0967614200967726, 0.3231945112351280, -0.8890410939633070, 
               -2.1449175681561337, 0.0969398706247005, 0.5715303576130624, 
               -0.3736063203227514, 2.4012648219173056, 1.6909171770109819, 
               1.2273184345746393, -0.5716867736574170, 2.4754127682646065, 
               1.8543058392220768};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 100, -1, 0, 0, false);

  ASSERT_NEAR(55.743706978278084, slp->f(z), EPS) << "Not correct objective";

  delete slp;
}

TEST_F(SLPTest, SLPF_FFTFILTERING) {

  int N = 7;
  FTYPE x[] = {0.0886221518141870,0.6100883986609733,-2.3085004845255379,
               -0.1766595206825720,1.6502980035082231,1.0484879097668793,
               -0.0877782968571668,2.7770486147025437,-0.3908467137394856,
               -0.8083623106713005,0.5207812917294559,0.9337442431429159,
               -1.0905918302151436,-0.5599398028754168};
  FTYPE z[] = {0.0967614200967726, 0.3231945112351280, -0.8890410939633070, 
               -2.1449175681561337, 0.0969398706247005, 0.5715303576130624, 
               -0.3736063203227514, 2.4012648219173056, 1.6909171770109819, 
               1.2273184345746393, -0.5716867736574170, 2.4754127682646065, 
               1.8543058392220768};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 100, -1, 0, 0, true);

  ASSERT_NEAR(55.743706978278084, slp->f(z), EPS) << "Not correct objective";

  delete slp;
}

TEST_F(SLPTest, SLPG) {

  int N = 7;
  FTYPE x[] = {0.0886221518141870,0.6100883986609733,-2.3085004845255379,
               -0.1766595206825720,1.6502980035082231,1.0484879097668793,
               -0.0877782968571668,2.7770486147025437,-0.3908467137394856,
               -0.8083623106713005,0.5207812917294559,0.9337442431429159,
               -1.0905918302151436,-0.5599398028754168};
  FTYPE z[] = {0.0967614200967726, 0.3231945112351280, -0.8890410939633070, 
               -2.1449175681561337, 0.0969398706247005, 0.5715303576130624, 
               -0.3736063203227514, 2.4012648219173056, 1.6909171770109819, 
               1.2273184345746393, -0.5716867736574170, 2.4754127682646065, 
               1.8543058392220768};
  FTYPE g[] = {-24.2617429059307526, -3.4303090631785649, -7.2364622011184432,
               -28.2157743079671626, -16.9380670468830523, 15.8575396577090526,
               -2.5176625893457683, 8.3601753090448288, 3.7514397297122186,
               2.1468723775284690, 3.7237638258135797, 1.5313004566769957, 
               -0.6021616682313282};

  FTYPE * gh = vector(2*N-1);

  slp_joint_iir * slp = new slp_joint_iir(N, x, 100, -1, 0, 0, false);

  slp->g(z, gh);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(g[i], gh[i], EPS) << "Vectors g and gh differ at index " << i;
  
  delete slp;
  del_vector(gh);
}


TEST_F(SLPTest, SLPG_FFTFILTERING) {

  int N = 7;
  FTYPE x[] = {0.0886221518141870,0.6100883986609733,-2.3085004845255379,
               -0.1766595206825720,1.6502980035082231,1.0484879097668793,
               -0.0877782968571668,2.7770486147025437,-0.3908467137394856,
               -0.8083623106713005,0.5207812917294559,0.9337442431429159,
               -1.0905918302151436,-0.5599398028754168};
  FTYPE z[] = {0.0967614200967726, 0.3231945112351280, -0.8890410939633070, 
               -2.1449175681561337, 0.0969398706247005, 0.5715303576130624, 
               -0.3736063203227514, 2.4012648219173056, 1.6909171770109819, 
               1.2273184345746393, -0.5716867736574170, 2.4754127682646065, 
               1.8543058392220768};
  FTYPE g[] = {-24.2617429059307526, -3.4303090631785649, -7.2364622011184432,
               -28.2157743079671626, -16.9380670468830523, 15.8575396577090526,
               -2.5176625893457683, 8.3601753090448288, 3.7514397297122186,
               2.1468723775284690, 3.7237638258135797, 1.5313004566769957, 
               -0.6021616682313282};

  FTYPE * gh = vector(2*N-1);

  slp_joint_iir * slp = new slp_joint_iir(N, x, 100, -1, 0, 0, true);

  slp->g(z, gh);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(g[i], gh[i], EPS) << "Vectors g and gh differ at index " << i;
  
  delete slp;
  del_vector(gh);
}


TEST_F(SLPTest, SLPPQ) {

  int N = 7;
  FTYPE x[] = {0.0886221518141870,0.6100883986609733,-2.3085004845255379,
               -0.1766595206825720,1.6502980035082231,1.0484879097668793,
               -0.0877782968571668,2.7770486147025437,-0.3908467137394856,
               -0.8083623106713005,0.5207812917294559,0.9337442431429159,
               -1.0905918302151436,-0.5599398028754168};
  FTYPE z[] = {0.0967614200967726, 0.3231945112351280, -0.8890410939633070, 
               -2.1449175681561337, 0.0969398706247005, 0.5715303576130624, 
               -0.3736063203227514, 2.4012648219173056, 1.6909171770109819, 
               1.2273184345746393, -0.5716867736574170, 2.4754127682646065, 
               1.8543058392220768};
  FTYPE y[] = {0.0000000000000000, 0.0000000000000000, -0.0000000000000000,
               -0.1000000000000001, 0.0000000000000000, 0.0000000000000000, 
               -0.0000000000000000, 0.0129260268263494, 0.0000000000000000,
                0.0000000000000000,-0.0000000000000000, 0.0870739731736503,
               0.0000000000000000};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 100, -1, 0.1, 0.1, false);

  slp->PQ(z);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(y[i], z[i], EPS) << "Vectors PQ and PQh differ at index " << i;
  
  delete slp;
}


TEST_F(SLPTest, SLP_JOINT_IIR1) {

  int N = 7;
  FTYPE x[] = {0.0886221518141870,0.6100883986609733,-2.3085004845255379,
               -0.1766595206825720,1.6502980035082231,1.0484879097668793,
               -0.0877782968571668,2.7770486147025437,-0.3908467137394856,
               -0.8083623106713005,0.5207812917294559,0.9337442431429159,
               -1.0905918302151436,-0.5599398028754168};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 10000, -1, 0.1, 0.1, false);

  FTYPE xk[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  FTYPE xs[] = {0, 0, 0, 0, 0.1, 0, 0.1, 0, 0, 0, 0, 0, 0};
  ofm(slp, xk);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(xk[i], xs[i], EPS) << "Vectors xk and x* differ at index " << i;
  
  delete slp;
}


TEST_F(SLPTest, SLP_JOINT_IIR1_FFTFILTERING) {

  int N = 7;
  FTYPE x[] = {0.0886221518141870,0.6100883986609733,-2.3085004845255379,
               -0.1766595206825720,1.6502980035082231,1.0484879097668793,
               -0.0877782968571668,2.7770486147025437,-0.3908467137394856,
               -0.8083623106713005,0.5207812917294559,0.9337442431429159,
               -1.0905918302151436,-0.5599398028754168};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 10000, -1, 0.1, 0.1, true);

  FTYPE xk[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  FTYPE xs[] = {0, 0, 0, 0, 0.1, 0, 0.1, 0, 0, 0, 0, 0, 0};
  ofm(slp, xk);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(xk[i], xs[i], EPS) << "Vectors xk and x* differ at index " << i;
  
  delete slp;
}



TEST_F(SLPTest, SLP_JOINT_IIR2) {

  int N = 20;
  FTYPE x[] = {0.0886221518141870, 0.6100883986609733, -2.3085004845255379, 
               -0.1766595206825720, 1.6502980035082231, 1.0484879097668793, 
               -0.0877782968571668, 2.7770486147025437, -0.3908467137394856,
               -0.8083623106713005, 0.5207812917294559, 0.9337442431429159, 
               -1.0905918302151436, -0.5599398028754168, 0.0967614200967726, 
               0.3231945112351280, -0.8890410939633070, -2.1449175681561337, 
               0.0969398706247005, 0.5715303576130624, -0.3736063203227514, 
               2.4012648219173056, 1.6909171770109819, 1.2273184345746393, 
               -0.5716867736574170, 2.4754127682646065, 1.8543058392220768, 
               0.6472268416140350, 0.9186476786180909, -0.7539092778173812, 
               -0.0143715193976287, 1.0552267645753632, 1.8083590507956371,
               0.1737987038017975, 1.0685078221563045, 0.3916771994447920,
               -1.3480289047069940, 1.6756366339920481, -0.3058778620827510,
               1.8917718872934635};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 10000, -1, 0.3, 0.4, false);

  ASSERT_NEAR(slp->L, 2.024428408161458e+02, EPS) << "Lh and L differs";

  FTYPE xk[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  FTYPE xs[] = {-0.0000000000000000,-0.0348776992149945,-0.0000000000000000,
                -0.0000000000000000,-0.0000000000000000,-0.0000000000000000,
                -0.0993041017255976,0.0000000000000000,-0.0000000000000000,
                -0.0000000000000000,-0.0000000000000000,-0.0000000000000000,
                0.0000000000000000,-0.0000000000000000,0.0000000000000000,
                -0.0487686073337265,-0.0000000000000000,-0.1170495917256815,
                0.0000000000000000, -0.0000000000000000,0.3019582429323436,
                0.0000000000000000,0.0000000000000000,-0.0000000000000000,
                0.0980417570676564,0.0000000000000000,0.0000000000000000,
                0.0000000000000000,-0.0000000000000000,-0.0000000000000000,
                0.0000000000000000,0.0000000000000000,0.0000000000000000,
                0.0000000000000000,0.0000000000000000,-0.0000000000000000,
                0.0000000000000000,-0.0000000000000000,0.0000000000000000};


  ofm(slp, xk);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(xk[i], xs[i], EPSIT) << "Vectors xk and x* differ at index " << i;
  
  delete slp;
}


TEST_F(SLPTest, SLP_JOINT_IIR2_FFTFILTERING) {

  int N = 20;
  FTYPE x[] = {0.0886221518141870, 0.6100883986609733, -2.3085004845255379, 
               -0.1766595206825720, 1.6502980035082231, 1.0484879097668793, 
               -0.0877782968571668, 2.7770486147025437, -0.3908467137394856,
               -0.8083623106713005, 0.5207812917294559, 0.9337442431429159, 
               -1.0905918302151436, -0.5599398028754168, 0.0967614200967726, 
               0.3231945112351280, -0.8890410939633070, -2.1449175681561337, 
               0.0969398706247005, 0.5715303576130624, -0.3736063203227514, 
               2.4012648219173056, 1.6909171770109819, 1.2273184345746393, 
               -0.5716867736574170, 2.4754127682646065, 1.8543058392220768, 
               0.6472268416140350, 0.9186476786180909, -0.7539092778173812, 
               -0.0143715193976287, 1.0552267645753632, 1.8083590507956371,
               0.1737987038017975, 1.0685078221563045, 0.3916771994447920,
               -1.3480289047069940, 1.6756366339920481, -0.3058778620827510,
               1.8917718872934635};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 10000, -1, 0.3, 0.4, true);

  ASSERT_NEAR(slp->L, 2.024428408161458e+02, EPS) << "Lh and L differs";

  FTYPE xk[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  FTYPE xs[] = {-0.0000000000000000,-0.0348776992149945,-0.0000000000000000,
                -0.0000000000000000,-0.0000000000000000,-0.0000000000000000,
                -0.0993041017255976,0.0000000000000000,-0.0000000000000000,
                -0.0000000000000000,-0.0000000000000000,-0.0000000000000000,
                0.0000000000000000,-0.0000000000000000,0.0000000000000000,
                -0.0487686073337265,-0.0000000000000000,-0.1170495917256815,
                0.0000000000000000, -0.0000000000000000,0.3019582429323436,
                0.0000000000000000,0.0000000000000000,-0.0000000000000000,
                0.0980417570676564,0.0000000000000000,0.0000000000000000,
                0.0000000000000000,-0.0000000000000000,-0.0000000000000000,
                0.0000000000000000,0.0000000000000000,0.0000000000000000,
                0.0000000000000000,0.0000000000000000,-0.0000000000000000,
                0.0000000000000000,-0.0000000000000000,0.0000000000000000};


  ofm(slp, xk);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(xk[i], xs[i], EPSIT) << "Vectors xk and x* differ at index " << i;
  
  delete slp;
}


TEST_F(SLPTest, SLP_JOINT_IIR3) {

  int N = 20;
  FTYPE x[] = {0.0886221518141870, 0.6100883986609733, -2.3085004845255379, 
               -0.1766595206825720, 1.6502980035082231, 1.0484879097668793, 
               -0.0877782968571668, 2.7770486147025437, -0.3908467137394856,
               -0.8083623106713005, 0.5207812917294559, 0.9337442431429159, 
               -1.0905918302151436, -0.5599398028754168, 0.0967614200967726, 
               0.3231945112351280, -0.8890410939633070, -2.1449175681561337, 
               0.0969398706247005, 0.5715303576130624, -0.3736063203227514, 
               2.4012648219173056, 1.6909171770109819, 1.2273184345746393, 
               -0.5716867736574170, 2.4754127682646065, 1.8543058392220768, 
               0.6472268416140350, 0.9186476786180909, -0.7539092778173812, 
               -0.0143715193976287, 1.0552267645753632, 1.8083590507956371,
               0.1737987038017975, 1.0685078221563045, 0.3916771994447920,
               -1.3480289047069940, 1.6756366339920481, -0.3058778620827510,
               1.8917718872934635};

  slp_joint_iir * slp = new slp_joint_iir(N, x, 100, -1, 0.3, 0.4, false);

  ASSERT_NEAR(slp->L, 2.024428408161458e+02, EPS) << "Lh and L differs";

  FTYPE xk[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
  FTYPE x100[] = {0.0000000000000000, -0.0349132827260680, -0.0000000000000000,
                  -0.0000000000000000, -0.0000000000000000,-0.0000000000000000,
                  -0.0991766135279938, 0.0000000000000000, -0.0000000000000000,
                  -0.0000000000000000, -0.0000000000000000,-0.0000000000000000,
                  0.0000000000000000,-0.0000000000000000,0.0000000000000000,
                  -0.0485332856641130, -0.0000000000000000,-0.1173768180818252,
                  0.0000000000000000, -0.0000000000000000, 0.3057506469267605,
                  0.0000000000000000, 0.0000000000000000, -0.0000000000000000,
                  0.0942493530732395, 0.0000000000000000, 0.0000000000000000,
                  0.0000000000000000, -0.0000000000000000, -0.0000000000000000,
                  0.0000000000000000, 0.0000000000000000, 0.0000000000000000,
                  0.0000000000000000, 0.0000000000000000, -0.0000000000000000,
                  0.0000000000000000, -0.0000000000000000, 0.0000000000000000};


  ofm(slp, xk);

  for( int i = 0 ; i < 2*N-1 ; i++ )
    ASSERT_NEAR(x100[i], xk[i], EPSIT) << "Vectors xk and x* differ at index " << i;
  
  delete slp;
}

int main(int argc, char **argv){
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();

}
